
/***************************************************************************** \
 *                     ColDiscreteDynamicsWorld
\*****************************************************************************/

/*! @file
 *
 *  @brief
 *
 *  @see
 *
 *  @author Weiyu Yi, wyi@tu-clausthal.de
 *
 */


//---------------------------------------------------------------------------
//  Includes
//---------------------------------------------------------------------------

#define COL_EXPORT

#include <col_import_export.h>
#include <ColDiscreteDynamicsWorld.h>
#include <ColWrapperBullet.h>

#include <ColExceptions.h>


namespace col {

//***************************************************************************
// ColDiscreteDynamicsWorld
//***************************************************************************

void ColDiscreteDynamicsWorld::addCollisionObject( btCollisionObject *colObj,
                                                   short int colFilterGroup ,
                                                   short int colFilterMask )
//void ColDiscreteDynamicsWorld::addRigidBody( btRigidBody *body )
{
    // 1) register the new object information into CollDet pipeline
    btGImpactMeshShape *shape = dynamic_cast<btGImpactMeshShape*>( colObj->getCollisionShape() );
    if ( shape == NULL )
    {
        throw XCollision( "Only btGImpactMeshShape supported\n" );
    }

    // 1.1) register geometry
    ColID objId = m_pipeline->makeCollidable( static_cast<void*>(shape), convertGeometryFromBulletShape );

    // 1.2) register transformation matrix
    btTransform trans = colObj->getWorldTransform();
    m_pipeline->moveObject( objId, static_cast<void*>(&trans), convertMatrixFromBulletTransform );

    // 1.3) register callback
    for ( unsigned int i=0; i < m_collisionObjects.size(); i ++ )
    {
        // create the corresponding manifold to save the collision detection
        // result, so that bullet can simulate the movement with dynamics
        // solver.
        btPersistentManifold *manifold = m_dispatcher1->getNewManifold( m_collisionObjects[i], colObj );

        // @care !
        // it is guaranteed that the ColID from CollDet pipeline is starting
        // at 0, it is the same with the index of btCollisionObject* in the
        // array, therefore we can use the btCollisionObject* index directly
        // as the ColID.
        m_pipeline->addCallback( new CreateContactCallback( i,
                                                            objId,
                                                            m_collisionObjects[i],
                                                            colObj,
                                                            manifold ) );
    }

    // 2) register the new object into bullet, so that it is available to the
    // dynamics solver.
    m_collisionObjects.push_back( colObj );
}

void ColDiscreteDynamicsWorld::performDiscreteCollisionDetection()
{
    BT_PROFILE("performDiscreteCollisionDetection");

    m_pipeline->check();
}

void ColDiscreteDynamicsWorld::internalSingleStepSimulation( btScalar timeStep )
{
    // call the original version
    btDiscreteDynamicsWorld::internalSingleStepSimulation(timeStep);

    // update the CollDet pipeline
    for ( unsigned int i=0; i < m_collisionObjects.size(); i++ )
    {
        btTransform trans = m_collisionObjects[i]->getWorldTransform();

        // @see ColDiscreteDynamicsWorld::addCollisionObject()
        // the index is the same as the ColID
        m_pipeline->moveObject( i, static_cast<void*>(&trans), convertMatrixFromBulletTransform );
    }
}


//***************************************************************************
// ColDiscreteDynamicsWorld::CreateContactCallback
//***************************************************************************

void ColDiscreteDynamicsWorld::CreateContactCallback::operator() ( const Data *data ) throw ()
{
    // todo
}

} // namespace col
